#include <gtest/gtest.h>
#include "../sample/navigation_task.h"
#include "../sample/battery_level_check.h"
#include "../sample/docking_task.h"
#include "scheduler/sequence_scheduler.h"
#include "scheduler/fallback_scheduler.h"
#include "task/task_chain.h"
#include "scheduler/scheduler_factory.h"

TEST(test_task_chain, test_task_chain_sequence)
{
    auto navigation_task = task::TaskFactory<NavigationTask>::createTask("navigation_task");
    auto battery_level_check = task::TaskFactory<BatteryLevelCheck>::createTask("battery_level_check");
    auto docking_task = task::TaskFactory<DockingTask>::createTask("docking_task");

    auto navigation_scheduler = scheduler::SchedulerFactory<scheduler::SequenceScheduler>::createScheduler("navigation_scheduler");
    navigation_scheduler->appendTask(battery_level_check);
    navigation_scheduler->appendTask(navigation_task);
//    navigation_scheduler->appendTask(docking_task);

    task::TaskChain taskChain("navi", navigation_scheduler);
    taskChain.start();

    EASE_INFO("done");
}

TEST(test_task_chain, test_task_chain_sequence_union)
{
    RobotPose robotPose{};
    robotPose.setX(12.3);
    robotPose.setY(23.4);
    robotPose.setYaw(34.5);

    auto navigation_task = task::TaskFactory<NavigationTask>::createTask("navigation_task");
    auto battery_level_check = task::TaskFactory<BatteryLevelCheck>::createTask("battery_level_check");
    auto docking_task = task::TaskFactory<DockingTask>::createTask("docking_task");

    auto navigation_scheduler = scheduler::SchedulerFactory<scheduler::SequenceScheduler>::createScheduler("navigation_scheduler");
    navigation_scheduler->appendTask(navigation_task);
    navigation_scheduler->appendTask(docking_task);

    auto recharge_scheduler = scheduler::SchedulerFactory<scheduler::FallbackScheduler>::createScheduler("recharge_scheduler");
    recharge_scheduler->appendTask(battery_level_check);
    recharge_scheduler->appendScheduler(navigation_scheduler);

    task::TaskChain taskChain("recharge", recharge_scheduler);
    taskChain.setData("robot_pose", robotPose);
    taskChain.start();

    EASE_INFO("done");
}

